#include "ServoControler.h"
#include "Servos.h"
#include <iostream>
#include <iomanip>

ServoControler::ServoControler(Servos * ActuatorGroup, int servo_id):
m_ActuatorGroup(ActuatorGroup),
m_servoId(servo_id),
m_Consigne(127),
m_LastConsigne(-1)
{
}

ServoControler::~ServoControler()
{
}

void ServoControler::SetServoSpeed(int speed)
{
	char SerialMsg[5] = { 0x80, 0x01, 0x01, static_cast<char>(m_servoId), static_cast<char>(speed)};

	m_ActuatorGroup->WriteToBus(SerialMsg, 5);
}

void ServoControler::SetServoPosition(int position)
{
	char SerialMsg[6] = { 0x80, 0x01, 0x03, static_cast<char>(m_servoId), static_cast<char>((static_cast<int>(position) & 0x80) >> 7), static_cast<char>(static_cast<int>(position) & 0x7F)};
	m_ActuatorGroup->WriteToBus(SerialMsg, 6);
}

void ServoControler::Iteration()
{
	if(static_cast<int>(m_Consigne) == m_LastConsigne)
		return;

	SetServoPosition(m_Consigne);
	m_LastConsigne = m_Consigne;
}
